#include <QString>
#include <QtTest>

#include <math/MSc_Location.h>

////////////////////////////////////////////////////////////////////////////////

class MSc_LocationTest : public QObject
{
    Q_OBJECT

public:

    MSc_LocationTest();

private Q_SLOTS:

    //void initTestCase();

    void testConstructor();

    void testGetX();
    void testGetY();
    void testGetZ();

    void testGetAlt();
    void testGetLat();
    void testGetLon();

    void testGetWGS();

    void testGetNorm_WGS();
    void testGetGrav_WGS();

    void testGetT_ned2wgs();
    void testGetT_wgs2ned();

    void testSetGeo();
    void testSetWGS();

    void testGetFromGeo();
    void testGetFromWGS();

    void testGetOmega_WGS();

    void testGeo2wgs();
    void testWgs2geo();
};

////////////////////////////////////////////////////////////////////////////////

MSc_LocationTest::MSc_LocationTest() {}

////////////////////////////////////////////////////////////////////////////////

//void MSc_LocationTest::initTestCase()
//{
//    QVERIFY( fabs( MSc_Location::m_a - 6378137.0           ) < 10.0e-3 );
//    QVERIFY( fabs( MSc_Location::m_f - 1.0 / 298.257223563 ) < 10.0e-9 );

//    QVERIFY( fabs( MSc_Location::m_b      - 6356752.3142       ) < 10.0e-3 );
//    QVERIFY( fabs( MSc_Location::m_a2     - MSc_Location::m_a*MSc_Location::m_a ) < 10.0e-3 );
//    QVERIFY( fabs( MSc_Location::m_b2     - MSc_Location::m_b*MSc_Location::m_b ) < 10.0e-3 );
//    QVERIFY( fabs( MSc_Location::m_e2     - 6.69437999014e-3   ) < 10.0e-8 );
//    QVERIFY( fabs( MSc_Location::m_e      - 8.1819190842622e-2 ) < 10.0e-7 );
//    QVERIFY( fabs( MSc_Location::m_eprim2 - 6.73949674228e-3   ) < 10.0e-8 );
//    QVERIFY( fabs( MSc_Location::m_eprim  - 8.2094437949696e-2 ) < 10.0e-7 );

//    QVERIFY( fabs( MSc_Location::m_GM      - 3986004.418e8    ) < 1.0 );
//    QVERIFY( fabs( MSc_Location::m_omega   - 7292115.0e-11    ) < 10.0e-10 );
//    QVERIFY( fabs( MSc_Location::m_gamma_e - 9.7803253359     ) < 10.0e-9 );
//    QVERIFY( fabs( MSc_Location::m_gamma_p - 9.8321849378     ) < 10.0e-9 );
//    QVERIFY( fabs( MSc_Location::m_k       - 0.00193185265241 ) < 10.0e-9 );
//    QVERIFY( fabs( MSc_Location::m_m       - 0.00344978650684 ) < 10.0e-9 );
//}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testConstructor()
{
    MSc_Location loc;

    QVERIFY( fabs( loc.getLat() ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() ) < 10.0e-9 );

    QVERIFY( fabs( loc.getX() - 6378137.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getY() ) < 10.0e-9 );
    QVERIFY( fabs( loc.getZ() ) < 10.0e-9 );

    MSc_Location loc1;

    loc1.setGeo( M_PI / 4.0, M_PI / 2.0, 100.0 );

    MSc_Location loc2( loc1 );

    QVERIFY( fabs( loc2.getLat() - M_PI / 4.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc2.getLon() - M_PI / 2.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc2.getAlt() - 100.0 ) < 10.0e-9 );

    MSc_Location loc3( &loc1 );

    QVERIFY( fabs( loc3.getLat() - M_PI / 4.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc3.getLon() - M_PI / 2.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc3.getAlt() - 100.0 ) < 10.0e-9 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGetX()
{
    MSc_Location loc;

    loc.setWGS( 6378137.1, 0.0, 0.0 );

    QVERIFY( fabs( loc.getX() - 6378137.1 ) < 10.0e-9 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGetY()
{
    MSc_Location loc;

    loc.setWGS( 0.0, 6378137.2, 0.0 );

    QVERIFY( fabs( loc.getY() - 6378137.2 ) < 10.0e-9 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGetZ()
{
    MSc_Location loc;

    loc.setWGS( 0.0, 0.0, 6378137.3 );

    QVERIFY( fabs( loc.getZ() - 6378137.3 ) < 10.0e-9 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGetAlt()
{
    MSc_Location loc;

    loc.setGeo( 0.0, 0.0, 100.0 );

    QVERIFY( fabs( loc.getAlt() - 100.0 ) < 10.0e-9 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGetLat()
{
    MSc_Location loc;

    loc.setGeo( M_PI / 4.0, 0.0, 0.0 );

    QVERIFY( fabs( loc.getLat() - M_PI / 4.0 ) < 10.0e-9 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGetLon()
{
    MSc_Location loc;

    loc.setGeo( 0.0, M_PI / 2.0, 0.0 );

    QVERIFY( fabs( loc.getLon() - M_PI / 2.0 ) < 10.0e-9 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGetWGS()
{
    MSc_Location loc;

    loc.setWGS( 6378137.1, 6378137.2, 6378137.3 );

    MSc_Vector3 wgs = loc.getWGS();

    QVERIFY( fabs( wgs( 0 ) - 6378137.1 ) < 10.0e-9 );
    QVERIFY( fabs( wgs( 1 ) - 6378137.2 ) < 10.0e-9 );
    QVERIFY( fabs( wgs( 2 ) - 6378137.3 ) < 10.0e-9 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGetNorm_WGS()
{
    MSc_Location loc;
    MSc_Vector3 norm;

    // 0N 0E
    loc.setGeo( 0.0, 0.0, 0.0 );

    norm = loc.getNorm_WGS();

    QVERIFY( fabs( norm( 0 ) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( norm( 1 ) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( norm( 2 ) - 0.0 ) < 10.0e-9 );

    // 0N 180E
    loc.setGeo( 0.0, M_PI, 0.0 );

    norm = loc.getNorm_WGS();

    QVERIFY( fabs( norm( 0 ) + 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( norm( 1 ) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( norm( 2 ) - 0.0 ) < 10.0e-9 );

    // 0N 90E
    loc.setGeo( 0.0, M_PI / 2.0, 0.0 );

    norm = loc.getNorm_WGS();

    QVERIFY( fabs( norm( 0 ) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( norm( 1 ) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( norm( 2 ) - 0.0 ) < 10.0e-9 );

    // 0N 90W
    loc.setGeo( 0.0, -M_PI / 2.0, 0.0 );

    norm = loc.getNorm_WGS();

    QVERIFY( fabs( norm( 0 ) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( norm( 1 ) + 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( norm( 2 ) - 0.0 ) < 10.0e-9 );

    // 90N 0E
    loc.setGeo( M_PI / 2.0, 0.0, 0.0 );

    norm = loc.getNorm_WGS();

    QVERIFY( fabs( norm( 0 ) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( norm( 1 ) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( norm( 2 ) - 1.0 ) < 10.0e-9 );

    // 90S 0E
    loc.setGeo( -M_PI / 2.0, 0.0, 0.0 );

    norm = loc.getNorm_WGS();

    QVERIFY( fabs( norm( 0 ) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( norm( 1 ) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( norm( 2 ) + 1.0 ) < 10.0e-9 );

    // 52.303425N 21.160708E :)
    double lat = M_PI * 52.303425 / 180.0;
    double lon = M_PI * 21.160708 / 180.0;
    loc.setGeo( lat, lon, 91.0 );

    norm = loc.getNorm_WGS();

    QVERIFY( fabs( norm( 0 ) - 0.570249 ) < 10.0e-5 );
    QVERIFY( fabs( norm( 1 ) - 0.220735 ) < 10.0e-5 );
    QVERIFY( fabs( norm( 2 ) - 0.79126  ) < 10.0e-5 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGetGrav_WGS()
{
    MSc_Location loc;
    MSc_Vector3 grav;

    double lat = 0.0;

    loc.setGeo( lat, 0.0, 0.0 );

    grav = loc.getGrav_WGS();

    QVERIFY( fabs( grav( 0 ) + 9.78 ) < 0.01 );
    QVERIFY( fabs( grav( 1 ) - 0.00 ) < 0.01 );
    QVERIFY( fabs( grav( 2 ) - 0.00 ) < 0.01 );

    loc.setGeo( lat, 0.0, 10000.0 );

    grav = loc.getGrav_WGS();

    QVERIFY( fabs( grav( 0 ) + 9.75 ) < 0.01 );
    QVERIFY( fabs( grav( 1 ) - 0.00 ) < 0.01 );
    QVERIFY( fabs( grav( 2 ) - 0.00 ) < 0.01 );

    lat = M_PI / 2.0;

    loc.setGeo( lat, 0.0, 0.0 );

    grav = loc.getGrav_WGS();

    QVERIFY( fabs( grav( 0 ) - 0.00 ) < 0.01 );
    QVERIFY( fabs( grav( 1 ) - 0.00 ) < 0.01 );
    QVERIFY( fabs( grav( 2 ) + 9.83 ) < 0.01 );

    loc.setGeo( lat, 0.0, 10000.0 );

    grav = loc.getGrav_WGS();

    QVERIFY( fabs( grav( 0 ) - 0.00 ) < 0.01 );
    QVERIFY( fabs( grav( 1 ) - 0.00 ) < 0.01 );
    QVERIFY( fabs( grav( 2 ) + 9.80 ) < 0.01 );

    lat = M_PI * 45.0 / 180.0;

    loc.setGeo( lat, 0.0, 0.0 );

    QVERIFY( fabs( loc.getGrav_WGS().getLength() - 9.81 ) < 0.01 );

    loc.setGeo( lat, 0.0, 5000.0 );

    QVERIFY( fabs( loc.getGrav_WGS().getLength() - 9.80 ) < 0.01 );

    loc.setGeo( lat, 0.0, 10000.0 );

    QVERIFY( fabs( loc.getGrav_WGS().getLength() - 9.78 ) < 0.01 );

    loc.setGeo( lat, 0.0, 15000.0 );

    QVERIFY( fabs( loc.getGrav_WGS().getLength() - 9.76 ) < 0.01 );

    loc.setGeo( lat, 0.0, 20000.0 );

    QVERIFY( fabs( loc.getGrav_WGS().getLength() - 9.74 ) < 0.01 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGetT_ned2wgs()
{
    MSc_Location loc;
    MSc_Matrix3x3 T_ned2wgs;

    // 0N 0E
    loc.setGeo( 0.0, 0.0, 0.0 );

    T_ned2wgs = loc.getT_ned2wgs();

    QVERIFY( fabs( T_ned2wgs(0,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(0,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(0,2) + 1.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_ned2wgs(1,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(1,1) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(1,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_ned2wgs(2,0) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(2,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(2,2) - 0.0 ) < 10.0e-9 );

    // 0N 180E
    loc.setGeo( 0.0, M_PI, 0.0 );

    T_ned2wgs = loc.getT_ned2wgs();

    QVERIFY( fabs( T_ned2wgs(0,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(0,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(0,2) - 1.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_ned2wgs(1,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(1,1) + 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(1,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_ned2wgs(2,0) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(2,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(2,2) - 0.0 ) < 10.0e-9 );

    // 0N 90E
    loc.setGeo( 0.0, M_PI / 2.0, 0.0 );

    T_ned2wgs = loc.getT_ned2wgs();

    QVERIFY( fabs( T_ned2wgs(0,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(0,1) + 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(0,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_ned2wgs(1,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(1,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(1,2) + 1.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_ned2wgs(2,0) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(2,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(2,2) - 0.0 ) < 10.0e-9 );

    // 0N 90W
    loc.setGeo( 0.0, -M_PI / 2.0, 0.0 );

    T_ned2wgs = loc.getT_ned2wgs();

    QVERIFY( fabs( T_ned2wgs(0,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(0,1) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(0,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_ned2wgs(1,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(1,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(1,2) - 1.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_ned2wgs(2,0) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(2,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(2,2) - 0.0 ) < 10.0e-9 );

    // 90N 0E
    loc.setGeo( M_PI / 2.0, 0.0, 0.0 );

    T_ned2wgs = loc.getT_ned2wgs();

    QVERIFY( fabs( T_ned2wgs(0,0) + 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(0,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(0,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_ned2wgs(1,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(1,1) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(1,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_ned2wgs(2,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(2,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(2,2) + 1.0 ) < 10.0e-9 );

    // 90S 0E
    loc.setGeo( -M_PI / 2.0, 0.0, 0.0 );

    T_ned2wgs = loc.getT_ned2wgs();

    QVERIFY( fabs( T_ned2wgs(0,0) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(0,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(0,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_ned2wgs(1,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(1,1) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(1,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_ned2wgs(2,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(2,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_ned2wgs(2,2) - 1.0 ) < 10.0e-9 );

    // 52.303425N 21.160708E :)
    double lat = M_PI * 52.303425 / 180.0;
    double lon = M_PI * 21.160708 / 180.0;
    loc.setGeo( lat, lon, 91.0 );

    T_ned2wgs = loc.getT_ned2wgs();

    QVERIFY( fabs( T_ned2wgs(0,0) + 0.737907 ) < 10.0e-5 );
    QVERIFY( fabs( T_ned2wgs(0,1) + 0.360985 ) < 10.0e-5 );
    QVERIFY( fabs( T_ned2wgs(0,2) + 0.570249 ) < 10.0e-5 );

    QVERIFY( fabs( T_ned2wgs(1,0) + 0.285633 ) < 10.0e-5 );
    QVERIFY( fabs( T_ned2wgs(1,1) - 0.932572 ) < 10.0e-5 );
    QVERIFY( fabs( T_ned2wgs(1,2) + 0.220735 ) < 10.0e-5 );

    QVERIFY( fabs( T_ned2wgs(2,0) - 0.611480 ) < 10.0e-5 );
    QVERIFY( fabs( T_ned2wgs(2,1) - 0.000000 ) < 10.0e-5 );
    QVERIFY( fabs( T_ned2wgs(2,2) + 0.791260 ) < 10.0e-5 );
}


////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGetT_wgs2ned()
{
    MSc_Location loc;
    MSc_Matrix3x3 T_wgs2ned;

    // 0N 0E
    loc.setGeo( 0.0, 0.0, 0.0 );

    T_wgs2ned = loc.getT_wgs2ned();

    QVERIFY( fabs( T_wgs2ned(0,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(0,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(0,2) - 1.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_wgs2ned(1,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(1,1) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(1,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_wgs2ned(2,0) + 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(2,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(2,2) - 0.0 ) < 10.0e-9 );

    // 0N 180E
    loc.setGeo( 0.0, M_PI, 0.0 );

    T_wgs2ned = loc.getT_wgs2ned();

    QVERIFY( fabs( T_wgs2ned(0,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(0,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(0,2) - 1.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_wgs2ned(1,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(1,1) + 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(1,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_wgs2ned(2,0) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(2,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(2,2) - 0.0 ) < 10.0e-9 );

    // 0N 90E
    loc.setGeo( 0.0, M_PI / 2.0, 0.0 );

    T_wgs2ned = loc.getT_wgs2ned();

    QVERIFY( fabs( T_wgs2ned(0,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(0,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(0,2) - 1.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_wgs2ned(1,0) + 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(1,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(1,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_wgs2ned(2,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(2,1) + 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(2,2) - 0.0 ) < 10.0e-9 );

    // 0N 90W
    loc.setGeo( 0.0, -M_PI / 2.0, 0.0 );

    T_wgs2ned = loc.getT_wgs2ned();

    QVERIFY( fabs( T_wgs2ned(0,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(0,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(0,2) - 1.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_wgs2ned(1,0) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(1,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(1,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_wgs2ned(2,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(2,1) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(2,2) - 0.0 ) < 10.0e-9 );

    // 90N 0E
    loc.setGeo( M_PI / 2.0, 0.0, 0.0 );

    T_wgs2ned = loc.getT_wgs2ned();

    QVERIFY( fabs( T_wgs2ned(0,0) + 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(0,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(0,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_wgs2ned(1,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(1,1) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(1,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_wgs2ned(2,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(2,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(2,2) + 1.0 ) < 10.0e-9 );

    // 90S 0E
    loc.setGeo( -M_PI / 2.0, 0.0, 0.0 );

    T_wgs2ned = loc.getT_wgs2ned();

    QVERIFY( fabs( T_wgs2ned(0,0) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(0,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(0,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_wgs2ned(1,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(1,1) - 1.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(1,2) - 0.0 ) < 10.0e-9 );

    QVERIFY( fabs( T_wgs2ned(2,0) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(2,1) - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( T_wgs2ned(2,2) - 1.0 ) < 10.0e-9 );

    // 52.303425N 21.160708E :)
    double lat = M_PI * 52.303425 / 180.0;
    double lon = M_PI * 21.160708 / 180.0;
    loc.setGeo( lat, lon, 91.0 );

    T_wgs2ned = loc.getT_wgs2ned();

    QVERIFY( fabs( T_wgs2ned(0,0) + 0.737907 ) < 10.0e-5 );
    QVERIFY( fabs( T_wgs2ned(0,1) + 0.285633 ) < 10.0e-5 );
    QVERIFY( fabs( T_wgs2ned(0,2) - 0.611480 ) < 10.0e-5 );

    QVERIFY( fabs( T_wgs2ned(1,0) + 0.360985 ) < 10.0e-5 );
    QVERIFY( fabs( T_wgs2ned(1,1) - 0.932572 ) < 10.0e-5 );
    QVERIFY( fabs( T_wgs2ned(1,2) - 0.000000 ) < 10.0e-5 );

    QVERIFY( fabs( T_wgs2ned(2,0) + 0.570249 ) < 10.0e-5 );
    QVERIFY( fabs( T_wgs2ned(2,1) + 0.220735 ) < 10.0e-5 );
    QVERIFY( fabs( T_wgs2ned(2,2) + 0.791260 ) < 10.0e-5 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testSetGeo()
{
    MSc_Location loc;

    loc.setGeo( M_PI / 4.0, M_PI / 2.0, 100.0 );

    QVERIFY( fabs( loc.getLat() - M_PI / 4.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() - M_PI / 2.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() - 100.0      ) < 10.0e-9 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testSetWGS()
{
    MSc_Location loc;
    MSc_Vector3 wgs1;
    MSc_Vector3 wgs2;

    wgs1( 0 ) = 4000000.0;
    wgs1( 1 ) = 5000000.0;
    wgs1( 2 ) = 6000000.0;

    loc.setWGS( wgs1 );

    wgs2 = loc.getWGS();

    QVERIFY( fabs( wgs2( 0 ) - 4000000.0 ) < 10.0e-9 );
    QVERIFY( fabs( wgs2( 1 ) - 5000000.0 ) < 10.0e-9 );
    QVERIFY( fabs( wgs2( 2 ) - 6000000.0 ) < 10.0e-9 );

    loc.setWGS( 4000001.0, 5000001.0, 6000001.0 );

    wgs2 = loc.getWGS();

    QVERIFY( fabs( wgs2( 0 ) - 4000001.0 ) < 10.0e-9 );
    QVERIFY( fabs( wgs2( 1 ) - 5000001.0 ) < 10.0e-9 );
    QVERIFY( fabs( wgs2( 2 ) - 6000001.0 ) < 10.0e-9 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGetFromGeo()
{
    MSc_Location loc;

    loc = MSc_Location::getFromGeo( M_PI / 4.0, M_PI / 2.0, 100.0 );

    QVERIFY( fabs( loc.getLat() - M_PI / 4.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() - M_PI / 2.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() - 100.0      ) < 10.0e-9 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGetFromWGS()
{
    MSc_Location loc;
    MSc_Vector3 wgs1;
    MSc_Vector3 wgs2;

    wgs1( 0 ) = 4000000.0;
    wgs1( 1 ) = 5000000.0;
    wgs1( 2 ) = 6000000.0;

    loc = MSc_Location::getFromWGS( wgs1 );

    wgs2 = loc.getWGS();

    QVERIFY( fabs( wgs2( 0 ) - 4000000.0 ) < 10.0e-9 );
    QVERIFY( fabs( wgs2( 1 ) - 5000000.0 ) < 10.0e-9 );
    QVERIFY( fabs( wgs2( 2 ) - 6000000.0 ) < 10.0e-9 );

    loc = MSc_Location::getFromWGS( 4000001.0, 5000001.0, 6000001.0 );

    wgs2 = loc.getWGS();

    QVERIFY( fabs( wgs2( 0 ) - 4000001.0 ) < 10.0e-9 );
    QVERIFY( fabs( wgs2( 1 ) - 5000001.0 ) < 10.0e-9 );
    QVERIFY( fabs( wgs2( 2 ) - 6000001.0 ) < 10.0e-9 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGetOmega_WGS()
{
    MSc_Vector3 omega_WGS = MSc_Location::getOmega_WGS();

    QVERIFY( fabs( omega_WGS( 0 ) ) < 10.0e-9 );
    QVERIFY( fabs( omega_WGS( 1 ) ) < 10.0e-9 );
    QVERIFY( fabs( omega_WGS( 2 ) - 7292115.0e-11 ) < 10.0e-11 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testGeo2wgs()
{
    MSc_Location loc;
    MSc_Vector3 wgs;

    // 0N 0E
    loc.setGeo( 0.0, 0.0, 0.0 );

    wgs = loc.getWGS();

    QVERIFY( fabs( wgs( 0 ) - 6378137.0 ) < 10.0e-3 );
    QVERIFY( fabs( wgs( 1 ) -       0.0 ) < 10.0e-3 );
    QVERIFY( fabs( wgs( 2 ) -       0.0 ) < 10.0e-3 );

    // 0N 180E
    loc.setGeo( 0.0, M_PI, 0.0 );

    wgs = loc.getWGS();

    QVERIFY( fabs( wgs( 0 ) + 6378137.0 ) < 10.0e-3 );
    QVERIFY( fabs( wgs( 1 ) -       0.0 ) < 10.0e-3 );
    QVERIFY( fabs( wgs( 2 ) -       0.0 ) < 10.0e-3 );

    // 0N 90E
    loc.setGeo( 0.0, M_PI / 2.0, 0.0 );

    wgs = loc.getWGS();

    QVERIFY( fabs( wgs( 0 ) -       0.0 ) < 10.0e-3 );
    QVERIFY( fabs( wgs( 1 ) - 6378137.0 ) < 10.0e-3 );
    QVERIFY( fabs( wgs( 2 ) -       0.0 ) < 10.0e-3 );

    // 0N 90W
    loc.setGeo( 0.0, -M_PI / 2.0, 0.0 );

    wgs = loc.getWGS();

    QVERIFY( fabs( wgs( 0 ) -       0.0 ) < 10.0e-3 );
    QVERIFY( fabs( wgs( 1 ) + 6378137.0 ) < 10.0e-3 );
    QVERIFY( fabs( wgs( 2 ) -       0.0 ) < 10.0e-3 );

    // 90N 0E
    loc.setGeo( M_PI / 2.0, 0.0, 0.0 );

    wgs = loc.getWGS();

    QVERIFY( fabs( wgs( 0 ) -       0.0000 ) < 10.0e-3 );
    QVERIFY( fabs( wgs( 1 ) -       0.0000 ) < 10.0e-3 );
    QVERIFY( fabs( wgs( 2 ) - 6356752.3142 ) < 10.0e-3 );

    // 90S 0E
    loc.setGeo( -M_PI / 2.0, 0.0, 0.0 );

    wgs = loc.getWGS();

    QVERIFY( fabs( wgs( 0 ) -       0.0000 ) < 10.0e-3 );
    QVERIFY( fabs( wgs( 1 ) -       0.0000 ) < 10.0e-3 );
    QVERIFY( fabs( wgs( 2 ) + 6356752.3142 ) < 10.0e-3 );

    // 52.303425N 21.160708E :)
    double lat = M_PI * 52.303425 / 180.0;
    double lon = M_PI * 21.160708 / 180.0;
    loc.setGeo( lat, lon, 91.0 );

    wgs = loc.getWGS();

    QVERIFY( fabs( wgs( 0 ) - 3644821.9369 ) < 10.0e-3 );
    QVERIFY( fabs( wgs( 1 ) - 1410858.4511 ) < 10.0e-3 );
    QVERIFY( fabs( wgs( 2 ) - 5023590.8735 ) < 10.0e-3 );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_LocationTest::testWgs2geo()
{
    MSc_Location loc;
    MSc_Vector3 wgs;

    // 0N 0E
    wgs.set( 6378137.0, 0.0, 0.0 );

    loc.setWGS( wgs(0), wgs(1), wgs(2) );

    QVERIFY( fabs( loc.getLat() - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() - 0.0 ) < 10.0e-3 );

    loc.setWGS( 1.0, 1.0, 1.0 );
    loc.setWGS( wgs );

    QVERIFY( fabs( loc.getLat() - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() - 0.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() - 0.0 ) < 10.0e-3 );

    // 0N 180E
    wgs.set( -6378137.0, 0.0, 0.0 );

    loc.setWGS( wgs(0), wgs(1), wgs(2) );

    QVERIFY( fabs( loc.getLat() -  0.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() - M_PI ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() -  0.0 ) < 10.0e-3 );

    loc.setWGS( 1.0, 1.0, 1.0 );
    loc.setWGS( wgs );

    QVERIFY( fabs( loc.getLat() -  0.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() - M_PI ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() -  0.0 ) < 10.0e-3 );

    // 0N 90E
    wgs.set( 0.0, 6378137.0, 0.0 );

    loc.setWGS( wgs(0), wgs(1), wgs(2) );

    QVERIFY( fabs( loc.getLat() -        0.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() - M_PI / 2.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() -        0.0 ) < 10.0e-3 );

    loc.setWGS( 1.0, 1.0, 1.0 );
    loc.setWGS( wgs );

    QVERIFY( fabs( loc.getLat() -        0.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() - M_PI / 2.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() -        0.0 ) < 10.0e-3 );

    // 0N 90W
    wgs.set( 0.0, -6378137.0, 0.0 );

    loc.setWGS( wgs(0), wgs(1), wgs(2) );

    QVERIFY( fabs( loc.getLat() -        0.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() + M_PI / 2.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() -        0.0 ) < 10.0e-3 );

    loc.setWGS( 1.0, 1.0, 1.0 );
    loc.setWGS( wgs );

    QVERIFY( fabs( loc.getLat() -        0.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() + M_PI / 2.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() -        0.0 ) < 10.0e-3 );

    // 90N 0E
    wgs.set( 0.0, 0.0, 6356752.3142 );

    loc.setWGS( wgs(0), wgs(1), wgs(2) );

    QVERIFY( fabs( loc.getLat() - M_PI / 2.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() -        0.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() -        0.0 ) < 10.0e-3 );

    loc.setWGS( 1.0, 1.0, 1.0 );
    loc.setWGS( wgs );

    QVERIFY( fabs( loc.getLat() - M_PI / 2.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() -        0.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() -        0.0 ) < 10.0e-3 );

    // 90S 0E
    wgs.set( 0.0, 0.0, -6356752.3142 );

    loc.setWGS( wgs(0), wgs(1), wgs(2) );

    QVERIFY( fabs( loc.getLat() + M_PI / 2.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() -        0.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() -        0.0 ) < 10.0e-3 );

    loc.setWGS( 1.0, 1.0, 1.0 );
    loc.setWGS( wgs );

    QVERIFY( fabs( loc.getLat() + M_PI / 2.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() -        0.0 ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() -        0.0 ) < 10.0e-3 );

    // 52.303425N 21.160708E :)
    wgs.set( 3644821.9369, 1410858.4511, 5023590.8735 );

    double lat = M_PI * 52.303425 / 180.0;
    double lon = M_PI * 21.160708 / 180.0;

    loc.setWGS( wgs(0), wgs(1), wgs(2) );

    QVERIFY( fabs( loc.getLat() -  lat ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() -  lon ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() - 91.0 ) < 10.0e-3 );

    loc.setWGS( 1.0, 1.0, 1.0 );
    loc.setWGS( wgs );

    QVERIFY( fabs( loc.getLat() -  lat ) < 10.0e-9 );
    QVERIFY( fabs( loc.getLon() -  lon ) < 10.0e-9 );
    QVERIFY( fabs( loc.getAlt() - 91.0 ) < 10.0e-3 );
}

////////////////////////////////////////////////////////////////////////////////

QTEST_APPLESS_MAIN(MSc_LocationTest)

////////////////////////////////////////////////////////////////////////////////

#include "tst_msc_locationtest.moc"
